package ncit.bluetooth.robots;

import ncit.bluetooth.connect.BluetoothChatService;


import android.os.Handler;
import android.util.Log;

public class LineFollower extends Robot {

	private int power;	//between 1000 and 1100
	private int direction; //between 2150 and 2850 , straight for 2500
	private static String TAG = "bluetooth.robots.LineFollower";
	
	public LineFollower()
	{
		Log.d(TAG,"instantiated line follower.");
		power = 1000;
		direction = 2500;
		
	}
	
	public LineFollower(Handler h){
		
		super(h);
		
	}
	
	
	@Override
	public void startMotors(){
		Log.i(TAG,"Entered startMotors.");
	
		byte p = 'p';
		BluetoothChatService.getInstance().write(p);
		
	}
	
	@Override
	public void startSensors(){
		Log.i(TAG,"Entered startSensors.");
		
	}
	
	@Override
	public void goForwardBackward(float slope){
		Log.i(TAG,"Entered goForwardBackward.");
		
		power = (short) ((slope + 1) * 125);
		
		sendMessage();
	}
	
	
	@Override
	public void motorBreak(){
		Log.i(TAG,"Entered motorBreak.");
		
		//power = 1000;
		//sendMessage();
		byte p = 'p';
		BluetoothChatService.getInstance().write(p);
		
	}
	
	@Override
	public void goLeftRight(float slope){
	
		Log.i(TAG,"Entered goLeftRight.");
		
		direction = (short) ((250 + 325 * (slope + 1)));
		sendMessage();
	}
	
	/**
	 * Sends the line follower with the parani device attached a message made out of 
	 * strings. 
	 */
	public void sendMessage(){
		
		String powerString = new String();
		
		powerString = "" + power;
		
		String angleString = new String();
		
		angleString = "" + direction;	
		
		char[] powerChars = powerString.toCharArray();
		char[] angleChars = angleString.toCharArray();
		
		
		BluetoothChatService.getInstance().write((byte)'s');
		for(int i = 0; i < powerChars.length; i++)
			BluetoothChatService.getInstance().write((byte)powerChars[i]);
		
		BluetoothChatService.getInstance().write((byte)'t');
		for(int i = 0; i < angleChars.length; i++)
			BluetoothChatService.getInstance().write((byte)angleChars[i]);
		
	}

	@Override
	public void askSensors() {
		
	}

	@Override
	public void park() {
		
	}
	
}
